#ifndef BTL_EXTRA_SIMULATION_POINTCLOUD
#define BTL_EXTRA_SIMULATION_POINTCLOUD

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/random/mersenne_twister.hpp>
#include <vector>

namespace btl
{
namespace extra
{
namespace simulation
{

class PointCloud
{
   public:
      Eigen::Matrix3Xd points;

      void setUniformBox(
         int N,
         const Eigen::Vector3d& minCoord,
         const Eigen::Vector3d& maxCoord,
         boost::mt19937& rng);
};

class Frame
{
   public:
      /// camera matrix for the frame
      Eigen::Isometry3d worldToCamera;
      /// feature rays
      Eigen::Matrix3Xd measurements;
      /// indexes of valid measurements
      std::vector<int> indexes;
};

class Sequence
{
   public:
      std::vector<Frame> frames;
};

class Motion
{
   public:
      std::vector<Eigen::Isometry3d> cameraToWorld;

      void setLinearMotion(int N, const Eigen::Isometry3d& start, const Eigen::Isometry3d& end);
};

class NoisyPointSensor
{
   public:
      /// noise (standard deviation in pixels) for inlier measurements
      double measurementNoise;
      /// minimum allowed z coordinate (if excludeBehindCamera is true)
      double nearPlane;
      /// horizontal field of view in radians
      double fovX;
      /// horizontal resolution of the sensor
      int resX;
      /// vertical resolution of the sensor
      int resY;
      /// exclude points that would lie outside the sensor frame
      bool excludeOutOfFrame;
      /// exclude points that lie behind the camera
      /// (i.e., points that have a negative z coordinate in camera space)
      bool excludeBehindCamera;

      NoisyPointSensor();

      Frame capture(
         const Eigen::Isometry3d& worldToCamera,
         const Eigen::Matrix3Xd& points,
         boost::mt19937& rng) const;
};

Eigen::Isometry3d lerpTransform(double alpha, const Eigen::Isometry3d& a, const Eigen::Isometry3d& b);

Sequence captureSequence(
      const PointCloud& world, const Motion& motion,
      const NoisyPointSensor& sensor, boost::mt19937& rng);

} // namespace simulation
} // namespace extra
} // namespace btl

// ====================================================================
// ------ Implementation

namespace btl
{
namespace extra
{
namespace simulation
{

inline Eigen::Isometry3d lerpTransform(
      double alpha, const Eigen::Isometry3d& a, const Eigen::Isometry3d& b)
{
   Eigen::Quaterniond rot0 = Eigen::Quaterniond(a.rotation());
   Eigen::Quaterniond rot1 = Eigen::Quaterniond(b.rotation());
   Eigen::Vector3d pos0 = a.translation();
   Eigen::Vector3d pos1 = b.translation();

   Eigen::Vector3d t = pos0 + alpha*(pos1 - pos0);
   Eigen::Isometry3d pose
      = Eigen::Translation3d(t)
      * Eigen::Isometry3d(rot0.slerp(alpha, rot1));

   return pose;
}

} // namespace simulation
} // namespace extra
} // namespace btl

#endif // BTL_EXTRA_SIMULATION_SIMULATION
